/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.SmartDashboard;

/**
 *
 * @author robotics
 */
public class SteerAngCounter implements PIDSource {
    Counter counter;
    String name;
    DigitalInput inputA;
    DigitalInput inputB;

    SteerAngCounter(int channelA, int channelB, String name)
    {
        System.out.println("SteerAngCounter::SteerAngCounter");
        this.name = name;
        inputA = new DigitalInput(4, channelA);
        inputB = new DigitalInput(4, channelB);
        counter = new Counter(EncodingType.k2X, inputA, inputB, false);
        counter.start();
    }

    public double pidGet()
    {
        double raw = counter.get()/2; // 720 counts per rev.
        double adjRaw = Utils.mod360(raw);
        SmartDashboard.log(raw, (name + " Real Raw"));
        SmartDashboard.log(adjRaw, (name + " Cooked"));

       // System.out.println("SteerAngCounter::pidGet adjRaw: "  + adjRaw);
        //  mod by 180 to get basic direction -- later we will deal with full 360 by reversing direction.
        //  int adjRaw = (raw%180) - 90;
       
        //  double adjRaw = Utils.limitValue(raw, 90);
        
        return adjRaw;
    }
}